﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Collections.ObjectModel;

using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

namespace Engine.Physics
{
    public class CollisionSolver : Manager
    {
        private List<ICollisionable> collisionables;

        static CollisionSolver _instance;
        public static CollisionSolver Instance { get { return _instance; } }

        private List<ICollisionable> collisions;
        public ReadOnlyCollection<ICollisionable> Collisions;

        public CollisionSolver()
        {
            _instance = this;

            collisionables = new List<ICollisionable>();
            collisions = new List<ICollisionable>(8);
            Collisions = new ReadOnlyCollection<ICollisionable>(collisions);
        }

        private static List<ICollisionable> previousCollisions;
        public override void Update(GameTime gameTime)
        {
            previousCollisions = new List<ICollisionable>(collisions);
            collisions.Clear();

            for (int i = 0; i < collisionables.Count; i++)
            {
                for (int j = i + 1; j < collisionables.Count; j++)
                {
                    ICollisionable a = collisionables[i];
                    ICollisionable b = collisionables[j];
                    

                    bool collides = false;
                    Vector3 collisionNormal = Vector3.Zero;
                    switch (a.Type)
                    {
                        case CollisionType.Sphere:
                            #region A is Sphere
                            switch (b.Type)
	                        {
                                case CollisionType.Sphere: collides = SphereToSphere(a, b, out collisionNormal);
                                    break;
                                case CollisionType.Box: collides = SphereToBox(a, b, out collisionNormal);
                                    break;
                                case CollisionType.Plane: throw new Exception("TODO");
                                default:
                                    break;
	                        }
                            #endregion
                            break;
                        case CollisionType.Box:
                            #region A is Box
                            {
                                switch (b.Type)
                                {
                                    case CollisionType.Sphere: collides = SphereToBox(a, b, out collisionNormal);
                                        break;
                                    case CollisionType.Box: collides = BoxToBox(a, b, out collisionNormal);
                                        break;
                                    case CollisionType.Plane:
                                        break;
                                    default:
                                        break;
                                }
                            }
                            #endregion
                            break;
                        case CollisionType.Plane:
                            break;
                        default:
                            break;
                    }

                    if (collides)
                    {
                        collisions.Add(a);
                        collisions.Add(b);

                        if (!previousCollisions.Contains(a))
                        {
                            OnCollision(a, b, collisionNormal);
                        }


                        OnColliding(a, b, collisionNormal);
                    }
                }
            }
            base.Update(gameTime);
        }

        private void OnColliding(ICollisionable a, ICollisionable b, Vector3 collisionNormal)
        {
            CollisionEventArgs aArgs = new CollisionEventArgs(b, collisionNormal);
            CollisionEventArgs bArgs = new CollisionEventArgs(a, -collisionNormal);
            a.OnColliding(aArgs);
            b.OnColliding(bArgs);
        }

        private void OnCollision(ICollisionable a, ICollisionable b, Vector3 collisionNormal)
        {
            CollisionEventArgs aArgs = new CollisionEventArgs(b, collisionNormal);
            CollisionEventArgs bArgs = new CollisionEventArgs(a, -collisionNormal);
            a.OnCollision(aArgs);
            b.OnCollision(bArgs);
        }

        private bool SphereToSphere(ICollisionable a, ICollisionable b, out Vector3 collisionNormal)
        {
            BoundingSphere aBS = a.Sphere.Transform(a.ParentWorld * a.World);
            BoundingSphere bBS = b.Sphere.Transform(b.ParentWorld * b.World);

            bool result = false;
            aBS.Intersects(ref bBS, out result);
            
            //Outputs
            collisionNormal = Vector3.Normalize(bBS.Center - aBS.Center);
            return result;

        }

        private static Vector3[] bboxCorners = new Vector3[8];
        private static Vector3[] transformedCorners = new Vector3[8];
        private bool SphereToBox(ICollisionable a, ICollisionable b, out Vector3 collisionNormal)
        {
            BoundingSphere aBS = a.Sphere.Transform(a.ParentWorld * a.World);
            BoundingBox bBox = b.BBox;

            bBox.GetCorners(bboxCorners);
            Matrix transform = b.ParentWorld * b.World;
            Vector3.Transform(bboxCorners, ref transform, transformedCorners);

            Vector3 min = transformedCorners[0];
            Vector3 max = min;

            for (int i = 1; i < 8; ++i)
			{
                Vector3 corner = transformedCorners[i];
			    min = Vector3.Min(min, corner);
                max = Vector3.Max(max, corner);
			}

            BoundingBox newBBox = new BoundingBox(min, max);

            bool result = false;
            aBS.Intersects(ref newBBox, out result);

            collisionNormal = new Vector3(0, 1, 0);
            return result;
        }

        private bool BoxToBox(ICollisionable a, ICollisionable b, out Vector3 collisionNormal)
        {
            collisionNormal = Vector3.UnitY;
            return false;
        }



        public void Subscribe(ICollisionable collisionable)
        {
            collisionables.Add(collisionable);
        }

        protected override void Dispose(bool disposing)
        {
            if (disposing)
            {
                collisionables.Clear();
            }
            base.Dispose(disposing);
        }
    }
}
